﻿using Ode.Net;
using Ode.Net.Collision;
using Ode.Net.Joints;
using OpenTK;

namespace Painter3D.PhysicalSys
{
    class ODEInterface
    {
        public static World world;
        public static Space space;
        public static JointGroup contactJointGroup;
        public void InitODE()
        {
            Engine.Init(InitFlags.None);
            world = new World();
            world.Gravity = new Ode.Net.Vector3(0f, -9.8f, 0f);
            world.QuickStepNumIterations = 50;
            space = new SimpleSpace();
            contactJointGroup = new JointGroup();
        }

        public void NearCallBack(Geom geom1, Geom geom2)
        {
            ContactGeom[] points = new ContactGeom[10];
            int count = Geom.Collide(geom1, geom2, points);
            ContactInfo contactInfo = new ContactInfo();
            for (int i = 0; i < count; i++)
            {
                contactInfo.Geometry = points[0];
                contactInfo.Surface.Mu = float.PositiveInfinity;
                
                Contact contact = new Contact(world, contactInfo, contactJointGroup);
                contact.Attach(geom1.Body, geom2.Body);
            }
        }

        public void Loop()
        {
            space.Collide(NearCallBack);
            world.Step(0.02f);
            contactJointGroup.Clear();
        }

        public void Destory()
        {
            Engine.Close();
        }

        public static OpenTK.Vector3 ODE2TK(Ode.Net.Vector3 vector3)
        {
            return new OpenTK.Vector3(vector3.X, vector3.Y, vector3.Z);
        }
        public static OpenTK.Vector4 ODE2TK(Ode.Net.Vector4 vector4)
        {
            return new OpenTK.Vector4(vector4.X, vector4.Y, vector4.Z, vector4.W);
        }
        public static OpenTK.Quaternion ODE2TK(Ode.Net.Quaternion qua)
        {
            return new OpenTK.Quaternion(qua.X, qua.Y, qua.Z, qua.W);
        }
        public static Ode.Net.Vector3 TK2ODE(OpenTK.Vector3 vector3)
        {
            return new Ode.Net.Vector3(vector3.X, vector3.Y, vector3.Z);
        }
        public static Ode.Net.Vector4 TK2ODE(OpenTK.Vector4 vector4)
        {
            return new Ode.Net.Vector4(vector4.X, vector4.Y, vector4.Z, vector4.W);
        }
        public static Ode.Net.Quaternion TK2ODE(OpenTK.Quaternion qua)
        {
            return new Ode.Net.Quaternion(qua.X, qua.Y, qua.Z, qua.W);
        }
    }
}
